// -*- coding: utf-8 -*-
#ifndef MISSION_H
#define MISSION_H

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <list>
#include <string>
#include "chassis.h"

enum ActionType
{
    OFFBOARD_ENABLED,
    MOVE_TO_POINT,
    ROTATE,
    TOPIC_CONTROL,
    SUPER_CONTROL,
    CIRCLE,
    POLE_SURROUND,
    HOVER,
    WAIT,
    SAFE_LANDING
};

struct Waypoint
{
    ActionType actionType;
    double x, y, z;
    double yaw;
    double duration;
    double speed;
    std::string stepName;
    std::string topicName;
    double radius;

    Waypoint(ActionType type, double x_val = 0, double y_val = 0, double z_val = 0,
             double yaw_val = 0, double duration_val = 0, double speed_val = 0.5,
             const std::string &name = "", const std::string &topic = "", double radius_val = 0);
};

class WaypointManager
{
private:
    std::list<Waypoint> waypoints;
    std::list<Waypoint>::iterator currentWaypoint;
    bool initialized;
    ros::Time missionStartTime;
    ros::Time actionStartTime;
    bool actionInProgress;
    double initialYaw;
    geometry_msgs::PoseStamped topicControlMsg;
    ros::Subscriber topicControlSub;
    ros::Publisher superControlPub;
    bool hasTopicControl;
    std::string currentTopicName;
    std::string lastStepName;
    ros::NodeHandle* nh_ptr;

public:
    WaypointManager();
    ~WaypointManager();
    
    void setNodeHandle(ros::NodeHandle* nh);
    void topicControlCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);
    void resetTopicControl();
    void setupTopicSubscription(ros::NodeHandle &nh, const std::string &topicName);
    bool hasNextWaypoint();
    Waypoint getCurrentWaypoint();
    void moveToNextWaypoint();
    void startCurrentAction();
    void resetHoldPosition();
    void resetRotationPosition();
    
    void addMovePoint(double x, double y, double z, const std::string &name, double speed=1.0);
    void addRotation(double yaw, const std::string &name);
    void addTopicControl(const std::string &topicName, double duration, const std::string &name);
    //x, y, z是目标航点，topicName是读取的super里的速度控制的信息，duration是持续时间，name是日志名称 
    void addSuperControl(double x, double y, double z, const std::string &topicName, double duration, const std::string &name); 
    void addCircleFlight(double centerX, double centerY, double centerZ, double radius, double duration, const std::string &name);
    void addPoleSurround(double pole_x, double pole_y, double radius, double height, bool clock_wise, const std::string &name);
    void addHover(double duration, const std::string &name);
    void addWait(double duration, const std::string &name);
    void addSafeLanding(const std::string &name);
    void addOffboardEnable(const std::string &name);
    void reset();
    void finalize();
    
    bool isCurrentActionComplete(move_map &move1, ros::Publisher &local_vel_pub, ros::Rate &rate);
};

#endif
